Velodyne VLP-32C激光雷达通信及数据解析

Posted on | In
Words count in article: 1,345

写了一段小程序用来进行简单环境下的VLP-32C激光雷达距离精度测试, 部分结果：



–> Velodyne VLP-32C Lidar user manual、测试环境与方法

demo 代码：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
"""
Velodyne VLP-32C激光雷达通信及数据解析程序
主要用来测试雷达的距离精度
author: 云水心
@2018.10
"""

import socket
import math
import time
import sys
from time import sleep

# UDP通信
s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
s.bind(('', 2368))
s.connect(('192.168.1.201', 2368))

# 传感器信息
marker = b'\xffee'
n_azimuth_bytes = 2
n_channels = 32
n_data_block_bytes = 100
n_channel_data_bytes = 3
n_data_blocks = 12
pitch_deg = [-25.0, -1.0, -1.667, -15.639, -11.31, 0.0, -0.667, -8.843,
             -7.254, 0.333, -0.333, -6.148, -5.333, 1.333, 0.667, -4.0,
             -4.667, 1.667, 1.0, -3.667, -3.333, 3.333, 2.333, -2.667,
             -3.0, 7.0, 4.667, 2.333, -2.0, 15.0, 10.333, -1.333]
delta_deg = [1.4, -4.2, 1.4, -1.4, 1.4, -1.4, 4.2, -1.4,
             1.4, -4.2, 1.4, -1.4, 4.2, -1.4, 4.2, -1.4,
             1.4, -4.2, 1.4, -4.2, 4.2, -1.4, 1.4, -1.4,
             1.4, -1.4, 1.4, -4.2, 4.2, -1.4, 1.4, -1.4]

deg2rad = math.pi / 180.0
rad2deg = 180.0 / math.pi
dist_meas = 4.0
s_name = str(dist_meas) + '.csv'  # 存储的文件名


# 输入三字节数据，返回距离值
def get_distance(data_per_channel):
    distance = (int(data_per_channel[1]) * 256 + int(data_per_channel[0])) * 4.0 / 1000.0
    return distance


# 输入channel值、距离值、航向角度，返回(x,y,z)
def get_xyz(channel, dist, azimuth):
    """
    x = dist * math.cos(pitch_deg[channel] * deg2rad) * math.sin((azimuth + delta_deg[channel]) * deg2rad)
    y = dist * math.cos(pitch_deg[channel] * deg2rad) * math.cos((azimuth + delta_deg[channel]) * deg2rad)
    z = dist * math.sin(pitch_deg[channel] * deg2rad)
    """
    x = dist * math.cos(pitch_deg[channel] * deg2rad) * math.sin(azimuth * deg2rad)
    y = dist * math.cos(pitch_deg[channel] * deg2rad) * math.cos(azimuth * deg2rad)
    z = dist * math.sin(pitch_deg[channel] * deg2rad)
    return x, y, z


# print(get_XYZ(21, 2.386, 0.16))  # (-0.051546641,2.381406307,0.138719708)


# 输入两字节数据，返回角度值
def get_azimuth(azimuth_data):
    angle = (int(azimuth_data[1]) * 256 + int(azimuth_data[0])) / 100.0
    return angle


def get_precision_azimuth(data_packet):  # 传引用
    i = 0
    azimuth_gap = 0.0
    for i in range(12):
        if i < 11:
            if data_packet[i + 1]['Azimuth'] < data_packet[i + 1]['Azimuth']:
                data_packet[i + 1]['Azimuth'] += 360
            azimuth_gap = data_packet[i + 1]['Azimuth'] - data_packet[i]['Azimuth']
        j = 0
        while j < 32:
            data_packet[i]['channel %d' % (j)][0] = data_packet[i]['Azimuth'] + azimuth_gap * j * 2.304 / 55.296 + \
                                                    delta_deg[j]
            data_packet[i]['channel %d' % (j + 1)][0] = data_packet[i]['Azimuth'] + azimuth_gap * j * 2.304 / 55.296 + \
                                                        delta_deg[j + 1]
            if data_packet[i]['channel %d' % (j)][0] > 360:
                data_packet[i]['channel %d' % (j)][0] -= 360

            elif data_packet[i]['channel %d' % (j)][0] < 0:
                data_packet[i]['channel %d' % (j)][0] += 360

            if data_packet[i]['channel %d' % (j + 1)][0] > 360:
                data_packet[i]['channel %d' % (j + 1)][0] -= 360

            elif data_packet[i]['channel %d' % (j + 1)][0] < 0:
                data_packet[i]['channel %d' % (j + 1)][0] += 360

            j += 2


# 每个data_packet包含12个data_block, data_block形式如下，每个channel包含azimuth和distance：
"""
data_block = {'Azimuth': .0,
              'channel 0': [.0, .0], 'channel 1': [.0, .0], 'channel 2': [.0, .0], ' channel 3': [.0, .0], 
              'channel 4': [.0, .0], 'channel 5': [.0, .0], 'channel 6': [.0, .0], 'channel 7': [.0, .0], 
              'channel 8': [.0, .0], 'channel 9': [.0, .0], 'channel 10': [.0, .0], 'channel 11': [.0, .0], 
              'channel 12': [.0, .0], 'channel 13': [.0, .0], 'channel 14': [.0, .0], 'channel 15': [.0, .0], 
              'channel 16': [.0, .0], 'channel 17': [.0, .0], 'channel 18': [.0, .0], 'channel 19': [.0, .0], 
              'channel 20': [.0, .0], 'channel 21': [.0, .0], 'channel 22': [.0, .0], 'channel 23': [.0, .0], 
              'channel 24': [.0, .0], 'channel 25': [.0, .0], 'channel 26': [.0, .0], 'channel 27': [.0, .0], 
              'channel 28': [.0, .0], 'channel 29': [.0, .0], 'channel 30': [.0, .0], 'channel 31': [.0, .0]}
"""


# 返回data packet
def getinfo(recv_Data):
    data_packet = []
    for i in range(n_data_blocks):
        data_block = dict()
        block_azimuth = get_azimuth(
            recv_Data[i * n_data_block_bytes + 2: i * n_data_block_bytes + 4])  # ffee标志位后的两字节为azimuth
        data_block.update({'Azimuth': block_azimuth})
        for j in range(n_channels):
            s_t = 'channel ' + str(j)
            dist = get_distance(recv_Data[
                                i * n_data_block_bytes + 4 + j * n_channel_data_bytes: i * n_data_block_bytes + 7 + j * n_channel_data_bytes])
            data_block.update({s_t: [block_azimuth, dist]})  # 此处尚未计及精确的azimuth
        data_packet.append(data_block)
    get_precision_azimuth(data_packet)
    return data_packet


with open(s_name, 'a') as f:
    f.write("time, Azimuth, channel 5 distance, channel5 X, channel5 Y, channel5 Z, error\n")
    f.close()

time_start = time.clock()

while True:
    recvData = s.recv(1248)  # 接收udp数据
    # print(recvData)
    li1 = []
    d_p = getinfo(recvData)  # 解析data_packet
    for i in range(12):
        # li = [d_p[i]['Azimuth']]
        t = d_p[i]['channel 5'][1]  # 默认为channel 5距离
        # 1-(-1), 5-(0), 9-(0.333),10-(-0.333), 18-(1), 考虑垂直放置误差，取用最短距离
        """
        if min(d_p[i]['channel 5'][1], d_p[i]['channel 9'][1], d_p[i]['channel 10'][1]) != 0:
            t = min(d_p[i]['channel 5'][1], d_p[i]['channel 9'][1], d_p[i]['channel 10'][1])
            # t = (d_p[i]['channel 5'][1] + d_p[i]['channel 9'][1] + d_p[i]['channel 10'][1]) / 3
        """
        # li = [d_p[i]['channel 5'][0], d_p[i]['channel 5'][1]]  # data_block的azimuth、channel5（0度俯仰角）的distance
        li = [d_p[i]['channel 5'][0], t]
        li1.append(li)  # li1包含12个data_block的azimuth、channel5（0度俯仰角）的distance
    for i in range(12):
        # print("Azimuth:", li1[i][0], "Channel distance", li1[i][1:33])
        if (li1[i][0] < 4.0 or li1[i][0] > 356.0) and li1[i][1] != .0:  # 限定航向角度范围
            with open(s_name, 'a') as f:
                time_elapse = time.clock() - time_start
                # 写入时间、角度、距离、X、Y、Z、error
                f.write("%f, %f, %f, %f, %f, %f, %f\n" % (
                    time_elapse, li1[i][0], li1[i][1], get_xyz(5, li1[i][1], li1[i][0])[0],
                    get_xyz(5, li1[i][1], li1[i][0])[1], get_xyz(5, li1[i][1], li1[i][0])[2],
                    get_xyz(5, li1[i][1], li1[i][0])[1] - dist_meas))
            f.close()
            print(time_elapse, "Azimuth:", li1[i][0], "channel 5 distance", li1[i][1], "channel 5 XYZ",
                  get_xyz(5, li1[i][1], li1[i][0]))
    # print("delta_Azimuth:%f" % (li1[11][0] - li1[10][0]))  # 5hz:0.09、0.1、0.11, 10hz:0.18、0.19、0.2 ！！！有跳动